#include "HangingCloth.h"

HangingCloth::HangingCloth(int numX, int numY, const Vector3d& base, const Vector3d& dirX, const Vector3d& dirY, ElasticMaterial& material) {
    this->numX = numX;
    this->numY = numY;
    this->base = base;
    this->dir = dirX;
    spacingX = dirX.magnitude();
    spacingY = dirY.magnitude();

    this->material = material;

    particles = new PointParticle[numX*numY];

    for (int i = 0; i < numY; i++) {
        for (int j = 0; j < numX; j++) {
            Vector3d pos = base + i * dirY + j * dirX;
            getParticle(j, i) = PointParticle(pos);
        }
    }
}

HangingCloth::~HangingCloth() {
    delete[] particles;
}

PointParticle& HangingCloth::getParticle(int x, int y) const {
    return particles[x+y*numX];
}

Vector3d& HangingCloth::getPos(int x, int y) const {
    return getParticle(x, y).pos;
}

Vector3d HangingCloth::getNormal(int x, int y) const {
    Vector3d dirX, dirY, result;
    if (y == 0) {
        dirY = getPos(x, 1) - getPos(x, 0);
    } else if (y == numY - 1) {
        dirY = getPos(x, numY - 1) - getPos(x, numY - 2);
    } else {
        dirY = getPos(x, y + 1) - getPos(x, y - 1);
    }

    if (x == 0) {
        dirX = getPos(1, y) - getPos(0, y);
    } else if (x == numX - 1) {
        dirX = getPos(numX - 1, y) - getPos(numX - 2, y);
    } else {
        dirX = getPos(x + 1, y) - getPos(x - 1, y);
    }

    result = dirY ^ dirX;
    result.normalize();
    return result;
}

void HangingCloth::render() const {
    for (int i = 0; i < numY - 1; i++) {
        glBegin(GL_QUAD_STRIP);
        for (int j = 0; j < numX; j++) {
            getNormal(j, i).glNormal();
            getParticle(j, i).pos.glVertex();
            getNormal(j, i+1).glNormal();
            getParticle(j, i+1).pos.glVertex();
        }
        glEnd();
    }
}

void HangingCloth::applyForces() {
    //vertical pull
    for (int i = 0; i < numY - 1; i++) {
        for (int j = 0; j < numX; j++) {
            getParticle(j, i).hookesLaw(material.ky, material.stiffnessDamping, spacingY, getParticle(j, i + 1));
        }
    }

    //horizontal pull
    for (int i = 1; i < numY; i++) {
        for (int j = 0; j < numX - 1; j++) {
            getParticle(j, i).hookesLaw(material.kx, material.stiffnessDamping, spacingX, getParticle(j + 1, i));
        }
    }

    //base forces
    for (int i = 1; i < numY; i++) {
        for (int j = 0; j < numX; j++) {
            getParticle(j, i).acc += material.g + material.massDamping * getParticle(j, i).vel;
        }
    }
}

void HangingCloth::eulerUpdate(double dt) {
    applyForces();

    //update
    for (int i = 1; i < numY; i++) {
        for (int j = 0; j < numX; j++) {
            getParticle(j, i).eulerUpdate(dt);
        }
    }
}

void HangingCloth::velocityVerletUpdate(double dt) {
    applyForces();

    //update
    for (int i = 1; i < numY; i++) {
        for (int j = 0; j < numX; j++) {
            getParticle(j, i).velocityVerletUpdate(dt);
        }
    }
}

void HangingCloth::move(const Vector3d& diff) {
    for (int j = 0; j < numX; j++) {
        getParticle(j, 0).pos += diff;
    }
}

void HangingCloth::rotate(double angleLeft, double angleUp) {
    dir.rotateLeft(angleLeft);
    dir.rotateUp(angleUp);

    for (int j = 0; j < numX; j++) {
        getParticle(j, 0).pos = base + dir * j;
    }
}
